#include "flysky.hpp"
#include "UI.hpp"

/**
 * @brief 对象初始化
 *
 */
FLYSKY::FLYSKY(void) {
    reset();
}

/**
 * @brief 初始化遥控器以及遥控器事件
 *
 */
void FLYSKY::init(void) {
    DMALIB_UART_Receive_IT(&DBUS_USART, data, FS_RECEIVE_SIZE);

    event     = rt_event_create("sbus", RT_IPC_FLAG_PRIO);
    fs_thread = rt_thread_create("fs", fs_thread_entry, (void *)this, 1024, 4, 10);

    rt_thread_startup(fs_thread);
}
/**
 * @brief 初始化遥控器
 *
 */
void FLYSKY::reset(void) {
		remoter.online = 0;
		remoter.ch[0] = remoter.ch[1] = remoter.ch[2] = remoter.ch[3] = remoter.ch[4] = 
		remoter.ch[5] = remoter.ch[6] = remoter.ch[7] = remoter.ch[8] = remoter.ch[9] = 0;
	
		remoter.left_x = remoter.left_y = remoter.right_x = remoter.right_y = 0;
		remoter.swa = remoter.swb = remoter.swc = remoter.swd = remoter.vra = remoter.vrb = 0;		
	
		rc_.left_x_ = rc_.left_y_ = rc_.right_x_ = rc_.right_y_ = rc_.wheel_ = 1024;
    rc_.left_sw_  = SW_DOWN;
    rc_.right_sw_ = SW_MID;
}

/**
 * @brief 解码遥控器数据
 *
 */
void FLYSKY::decode(void)
{
    if ((data[0] != SBUS_HEAD) || (data[24] != SBUS_END))
        return;

    if (data[23] == 0x0C) {
        remoter.online = 0;
				return;
		} else {
        remoter.online = 1;
				rt_event_send(event, FS_EVENT_ONLINE);
		}
		/* origin channal */
    remoter.ch[0] = ((data[1] | data[2] << 8) & 0x07FF);
    remoter.ch[1] = ((data[2] >> 3 | data[3] << 5) & 0x07FF);
    remoter.ch[2] = ((data[3] >> 6 | data[4] << 2 | data[5] << 10) & 0x07FF);
    remoter.ch[3] = ((data[5] >> 1 | data[6] << 7) & 0x07FF);
    remoter.ch[4] = ((data[6] >> 4 | data[7] << 4) & 0x07FF);
    remoter.ch[5] = ((data[7] >> 7 | data[8] << 1 | data[9] << 9) & 0x07FF);
    remoter.ch[6] = ((data[9] >> 2 | data[10] << 6) & 0x07FF);
    remoter.ch[7] = ((data[10] >> 5 | data[11] << 3) & 0x07FF);
    remoter.ch[8] = ((data[12] | data[13] << 8) & 0x07FF);
    remoter.ch[9] = ((data[13] >> 3 | data[14] << 5) & 0x07FF);
		/* stick & swtich & wheeel */
		remoter.left_x  = remoter.ch[3];
		remoter.left_y  = remoter.ch[2];
		remoter.right_x = remoter.ch[0];
		remoter.right_y = remoter.ch[1];
		
		remoter.vra = remoter.ch[4];
		remoter.vrb = remoter.ch[5];
		
		remoter.swa = remoter.ch[6];
		remoter.swb = remoter.ch[7];
		remoter.swc = remoter.ch[8];
		remoter.swd = remoter.ch[9];
		/* 整理为dr16格式 */
		fs_to_dr16();
}

/**
 * @brief 在线事件接收
 *
 * @return rt_err_t
 */
rt_err_t FLYSKY::event_recv(void) {
    return rt_event_recv(event, FS_EVENT_ONLINE, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 50, RT_NULL);
}

/**
 * @brief 遥控线程
 *
 * @param parameter
 */
void FLYSKY::fs_thread_entry(void *parameter) {
    rt_err_t ret;
    FLYSKY    *t_fs = (FLYSKY *)parameter;

    while (1) {
        ret = t_fs->event_recv();

        /* 遥控器断开连接 */
        if (ret != RT_EOK) {
            t_fs->reset();
					ui.ctrl=2;
        }
        // t_fs->debug();
    }
}

/**
 * @brief 访问遥控器数据
 *
 * @return struct __RC
 */
__RC FLYSKY::get_fs_rc(void) {
    return rc_;		
}

/**
 * @brief 访问10通道数据
 *
 * @return remoter
 */
__FS_RC FLYSKY::get_fs_orin(void) {
    return remoter;		
}

/**
 * @brief 整理为dr16格式
 *
 */
void FLYSKY::fs_to_dr16(void) {
		if (remoter.left_x < 1685 && remoter.left_x > 363)
			rc_.left_x_ = remoter.left_x;
		if (remoter.left_y < 1685 && remoter.left_y > 363)
			rc_.left_y_ = remoter.left_y;
		if (remoter.right_x < 1685 && remoter.right_x > 363)
			rc_.right_x_ = remoter.right_x;
		if (remoter.right_y < 1685 && remoter.right_y > 363)
			rc_.right_y_ = remoter.right_y;
	
		switch (remoter.swc) {
			case 240  : rc_.right_sw_ = SW_UP;		 break;
			case 1024 : rc_.right_sw_ = SW_MID;	   break;
			case 1807 : rc_.right_sw_ = SW_DOWN;   break;
			default   : rc_.right_sw_ = SW_MID;
		}
		
		switch (remoter.swb) {
			case 240  : rc_.left_sw_ = SW_MID; 		  break;
			case 1807 : rc_.left_sw_ = SW_UP;       break;
			default   : rc_.left_sw_ = SW_DOWN;
		}
		
		if (remoter.swa == 240)
			rc_.left_sw_ = SW_DOWN;
		
		if (remoter.vra < 1685 && remoter.vra > 363)
		  rc_.wheel_ = remoter.vra;
}

/**
 * @brief 调试函数
 *
 */
void FLYSKY::debug(void) {
    for (int i = 0; i < 10; i++) {
        rt_kprintf("%04d ", remoter.ch[i]);
    }
    rt_kprintf("\n");
}
